/*
Auteur        :Valentijn Polder
Studentnummer :0782531

$Log$

*/
#include "PID.h"

double uk1,q0,q1,q2,ek,ek1,ek2,P,I,D,T,gewenst,gemeten;
unsigned int duty;

void PID_Init(void){
   duty=0;
   uk1=0.0;
   q0=0.0;
   q1=0.0;
   q2=0.0;
   ek=0.0;
   ek1=0.0;
   ek2=0.0;
   P=1.0;
   I=65535.0;
   D=0.0;
   T=0.02;
   gewenst=30.0;
   gemeten=0.0;
}

void PID_Regel(void){
 uk1=duty;
 ek = gewenst-gemeten;
 
 q0=P*(1+(D/T));
 q1=P*((T/I)-1-(((2*D)/T)));
 q2=(P*D)/T;
 duty=(unsigned int)(uk1+q0*ek+q1*ek1+q2*ek2);

 ek2=ek1;
 ek1=ek;
}

/***********************

U[k] = U[k-1] + delta(U[k])

delta(U[k]) = delta(P[k]) + delta(I[k]) + delta(D[k])



*/


int type_A(int value){
  signed int u;
  
  
  
  lastValue =value;
  return(u);
}

int proportional(int k){
  return(e[k] - e[k-1]);
}

int differential(int k){
  switch type{
    case A:
      
  }
}

/** @fn     integrator(mode, k)
    @brief  calculates the delta[integration] component of a PID controller
    @param  mode  the discretization mode (FRM = 0, BRM = 1, TRAP = 2)
    @param  k     the value for k (maximum amount of samples)
    @return the value of the integration operation
    */
int integrator(int mode, int k){
  if(k < 1){return(NULL);}
  
  int nVal = 0;
  
  switch mode{
    //FRM: e[k-1]
    case 0:
      nVal = sample[1];
      break;
    //BRM: e[k]
    case 1:
      nVal = sample[0];
      break;
    //TRAP: (e[k] - e[k-1])/2 + e[k]
    case 2:
      nVal = ((sample[0] - sample[1])/2) + sample[0];
      break;
    default:
      break;
  }
  
  return(nVal);
}